#include "libmri.h"

#include <math.h>
#include <stdlib.h>
#include <iostream>
using std::cout;
using std::endl;
using std::cerr;
using std::flush;


static bool const verbose = false;

void
calib3d_lanczos (cplxf *&V, int& nv, cplxf *calib, int *csize, int *ksize, int k, float tau)
{
	double beta_tol = 1e-6;

	int nch = csize[3],
	    ks1 = ksize[0],
	    ks2 = ksize[1],
	    ks3 = ksize[2];

	int n = ks1 * ks2 * ks3 * nch;

	cplxf *alpha = new cplxf [k];
	float *beta = new float [k];
	cplxf *Q = new cplxf [n*k];
	float *D = new float [n];

	for (int i = 0; i < n*k; ++i){ Q[i].real = Q[i].imag = 0.0f; }
	for (int i = 0; i < k; ++i){ alpha[i] = 0.0f; }
	for (int i = 0; i < k; ++i){ beta[i] = 0.0f; }

	cplxf *b = new cplxf[n];
	for (int i = 0; i < n; ++i){
		b[i].real = drand48();
		b[i].imag = drand48();
	}

	double bnrm = 0; for (int i = 0; i < n; ++i){ bnrm += abs2 (b[i]); }
	bnrm = sqrt(bnrm);
	for (int i = 0; i < n; ++i){
		Q[i] = b[i] * (1.0 / bnrm);
	}

	cplxf *z = new cplxf[n];
	cplxf *w = new cplxf[n];

	double ts_iter = timestamp();

	cout << "Lanczos Iterations " << flush;
	for (int j = 0; j < k; ++j)
	{

		calib3d_atamul (z, calib, csize, &Q[j*n], ksize);

		alpha[j] = cdotc (n, (complex*) &Q[j*n], 1, (complex*) z, 1);

		for (int t = 0; t < 2; ++t) // reorthogonalize {\emph twice}!
		{
			complex _1[1] = {{ 1.0f, 0.0f}},
			        _0[1] = {{ 0.0f, 0.0f}},
			       _n1[1] = {{-1.0f, 0.0f}};

			cgemv ('C', n, j+1,  _1, (complex*) Q, n, (complex*) z, 1,
			        _0, (complex*) w, 1);

			cgemv ('N', n, j+1, _n1, (complex*) Q, n, (complex*) w, 1,
			        _1,	(complex*) z, 1);
		}

		double znrm = 0;
		for (int i = 0; i < n; ++i){ znrm += abs2 (z[i]); }
		znrm = sqrt (znrm);

		beta[j] = znrm;

		if (fabsf (beta[j]) < beta_tol) break;

		if (((j+1)%100) == 0) cout << " . " << fabsf(beta[j]) << flush;

		if (j < k-1){
			for (int i = 0; i < n; ++i)
				Q[i + n*(j+1)] = z[i] * (1.0 / znrm);
		}
	}
	cout << "lanczos iteration time: " << (timestamp() - ts_iter) << endl;

	//debug_shm ("alpha", alpha, 2, k, 1);
	//debug_shm ("beta", beta, 2, k, 1);
	//debug_shm ("Q", Q, 2, n, k);

	for (int i = 0; i < n; ++i){ D[i] = alpha[i].real; }
	int info = 0;

	double ts = timestamp();

	csteqr ('V', k, (float*) D, beta, (complex*) Q, n, &info);

	cout << "csteqr time: " << (timestamp() - ts) << endl;

	if (info != 0){
		cerr << "csteqr info=>" << info << endl;
	}

	//debug_shm ("D", D, 2, k, 1);
	//debug_shm ("V", Q, 2, n, k);

	float maxD = fabsf (D[0]);

	for (int i = 1; i < k; ++i)
		if (fabsf (D[i]) > maxD)
			maxD = fabsf (D[i]);

	nv = 0; int *vi = new int[k];
	for (int i = 0; i < k; ++i)
		if (fabsf (D[i]) >= tau*maxD)
			vi[nv++] = i;

	V = new cplxf [nv*n];

	for (int j = 0; j < nv; ++j)
		for (int i = 0; i < n; ++i)
			V[i + n*j] = Q [i + n*vi[j]];

	delete [] z;
	delete [] w;
	delete [] alpha;
	delete [] beta;
	delete [] Q;
	delete [] D;
	delete [] b;
}
